#include "tools.h"
#include <iostream>

#ifdef __linux__		
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#endif

const float SHRINKING_FACTOR = 7.5f;
const int NO_PROGRESS_STREAK_THRESHOLD = 100;

void Pcshower(std::vector<PointCloud> Vpc,bool showxyzaxis,std::string windowsname){

    open3d::visualization::Visualizer vis;
    vis.CreateVisualizerWindow(windowsname, 1280, 1024);
    if(showxyzaxis){
        Eigen::Vector3d origin(0.0, 0.0, 0.0);

        // X-axis in red (1, 0, 0)
        Eigen::Vector3d x_axis(1000.0, 0.0, 0.0);

        // Y-axis in green (0, 1, 0)
        Eigen::Vector3d y_axis(0.0, 1000.0, 0.0);

        // Z-axis in blue (0, 0, 1)
        Eigen::Vector3d z_axis(0.0, 0.0, 1000.0);

        // Create LineSet for each axis
        open3d::geometry::LineSet line_set_x;
        line_set_x.points_.push_back(origin);
        line_set_x.points_.push_back(x_axis);
        line_set_x.lines_.push_back({0, 1});
        line_set_x.colors_.push_back(Eigen::Vector3d(1.0, 0.0, 0.0));//x轴为红色

        open3d::geometry::LineSet line_set_y;
        line_set_y.points_.push_back(origin);
        line_set_y.points_.push_back(y_axis);
        line_set_y.lines_.push_back({0, 1});
        line_set_y.colors_.push_back(Eigen::Vector3d(0.0, 1.0, 0.0));//y轴为绿色

        open3d::geometry::LineSet line_set_z;
        line_set_z.points_.push_back(origin);
        line_set_z.points_.push_back(z_axis);
        line_set_z.lines_.push_back({0, 1});
        line_set_z.colors_.push_back(Eigen::Vector3d(0.0, 0.0, 1.0));//z轴为蓝色
        std::shared_ptr<open3d::geometry::LineSet> ptrx (new open3d::geometry::LineSet);
        *ptrx = line_set_x;   
        std::shared_ptr<open3d::geometry::LineSet> ptry (new open3d::geometry::LineSet);
        *ptry = line_set_y;
        std::shared_ptr<open3d::geometry::LineSet> ptrz (new open3d::geometry::LineSet);
        *ptrz = line_set_z;

        vis.AddGeometry(ptrx);
        vis.AddGeometry(ptry);
        vis.AddGeometry(ptrz);
    }




    for(PointCloud& a:Vpc){
        std::shared_ptr<PointCloud> pc_ptr(new PointCloud);
        pc_ptr->points_ = a.points_;
        pc_ptr->colors_ = a.colors_;
        vis.AddGeometry(pc_ptr);
        auto opt = vis.GetRenderOption();
        opt.point_size_ = 2.0;
        
    }




    vis.Run();
    vis.DestroyVisualizerWindow();
}

//矩阵求开方
MatrixXX Matirx_sq(MatrixXX M){
    Eigen::JacobiSVD<MatrixXX> svd(M,Eigen::ComputeThinU|Eigen::ComputeThinV);
    MatrixXX U = svd.matrixU();
    MatrixXX V = svd.matrixV();
    MatrixXX S = svd.singularValues().asDiagonal();
    MatrixXX S_sqrt = S.array().sqrt().matrix();

    MatrixXX A_sqrt = U*S_sqrt*V.transpose();

    return A_sqrt;

}

//矩阵求倒数
MatrixXX Matrix_inver(MatrixXX M){
    MatrixXX M_inver;
    for(int i=0;i<M.rows();i++){
        for(int j=0;i<M.cols();j++){
            M_inver(i,j)=1.0/M(i,j);
        }
    }
    return M_inver;
}

//点云下采样函数
PointCloud DownSample(double voxel_size,PointCloud pointcloud0){

    std::shared_ptr<open3d::geometry::PointCloud> pcloud0_down(new open3d::geometry::PointCloud);
    pcloud0_down = pointcloud0.VoxelDownSample(voxel_size);
    PointCloud pointcloud_down = *pcloud0_down;
    return pointcloud_down;


}

//离群点去除
PointCloud OutRemovel(int nb_points, double radius,PointCloud pointcloud0){

    std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>> result;
    result = pointcloud0.RemoveRadiusOutliers(nb_points,radius);
    std::shared_ptr<PointCloud> pcdown = std::get<0>(result);
    // auto index = std::get<1>(result);
    PointCloud pdresult = *pcdown;
    return pdresult;

}

PointCloud OutRemovel(int nb_points, double radius,PointCloud pointcloud0,std::vector<size_t> &index){

    std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>> result;
    result = pointcloud0.RemoveRadiusOutliers(nb_points,radius);
    std::shared_ptr<PointCloud> pcdown = std::get<0>(result);
    index = std::get<1>(result);
    PointCloud pdresult = *pcdown;
    return pdresult;

}

//扩展的哈希编码
int Scalhash(int G,Eigen::Vector3i grid_coor){
    int hashvalue;
    int x = grid_coor(0);
    int y = grid_coor(1);
    int z = grid_coor(2);

    //计算全局offset
    int offset_global;
    auto x_ = abs(x);auto y_ = abs(y);auto z_ = abs(z);
    int maxi;
    if(x_>=y_){
        if(x_>=z_){maxi = x;}
        else{maxi = z;}
    }
    else{
        if(y_>=z_){maxi = y;}
        else{maxi = z;}
    }
    if(maxi>0){
        offset_global = floor(2*maxi/G)+1;
    }
    else{
        offset_global = floor((-2*maxi-1)/G)+1;
    }

    //计算局部offset
    int sum = 0;
    for(int k =1;k<offset_global-1;k++){
        sum += pow(k,3);
    }
    int offset_local = pow(G,3)*sum;

    //计算hash值
    int g_x = x + G*offset_global*0.5;
    int g_y = y + G*offset_global*0.5;
    int g_z = z + G*offset_global*0.5;
    
    hashvalue = offset_local + g_x + g_y*G + g_z*G*G;
    return hashvalue;
}

int ghash(int G, Eigen::Vector3i grid_coor){
    
    return grid_coor(0)+grid_coor(1)*G+grid_coor(2)*G*G;
}

Eigen::Vector3i unhash(int value_hash,int G){
    int G2 = G*G;
    int x,y,z;

    z = floor(value_hash/(G2));
    y = floor((value_hash-z*G2)/G);
    x = value_hash-z*G2-y*G;
    Eigen::Vector3i coor;
    coor<<x,y,z;
    return coor;

}

open3d::geometry::Image o3d_cv(const cv::Mat& A) 
{
    open3d::geometry::Image B;
    int bytes_per_channel = (A.depth() / 2 + 1);//refer to the fuction depth
    B.Prepare(A.cols, A.rows, A.channels(), bytes_per_channel);
    std::memcpy(B.data_.data(), A.data, A.total() * A.channels() * bytes_per_channel);
    return B;
}

void cor_line(std::vector<cv::Point> a,std::vector<cv::Point> b,cv::Mat img1, cv::Mat img2){
    
    for(int i =0;i<a.size();i++){
        if(i%2000==0){
            cv::circle(img1,a[i],5,cv::Scalar(255,0,0),-1);
            cv::circle(img2,b[i],5,cv::Scalar(255,0,0),-1);
        }

    }
    cv::Mat image3 = cv::Mat::zeros(std::max(img1.rows, img2.rows), img1.cols + img2.cols, CV_8UC3);
    image3.setTo(cv::Scalar(255, 255, 255));
    img1.copyTo(image3(cv::Rect(0, 0, img1.cols, img1.rows)));
    img2.copyTo(image3(cv::Rect(img1.cols, 0, img2.cols, img2.rows)));
    for(int i =0;i<a.size();i++){
        if(i%2000==0){
            cv::line(image3, a[i], cv::Point(img1.cols + b[i].x, b[i].y), cv::Scalar(255, 0, 0), 2);
        }
        
    }
    cv::imshow("Points and Line", image3);
    cv::resizeWindow("Points and Line", 2000, 1000);
    // Wait for a key press and then close the window
    cv::waitKey(0);
    cv::destroyAllWindows();


}

void saveMatrixToTxt(const Eigen::MatrixXd& matrix, const std::string& filename) {
    std::ofstream file(filename);
    if (file.is_open()) {
        file << matrix;
        file.close();
        std::cout << "Matrix saved to " << filename << " successfully." << std::endl;
    } else {
        std::cerr << "Error opening file: " << filename << std::endl;
    }
}

// 最远点采样
std::vector<size_t>FarthestPointDownSample(std::shared_ptr<open3d::geometry::PointCloud>& cloud,size_t sampleNum)
{
	if (sampleNum == 0) 
	{
		open3d::utility::LogWarning("采样点个数不能为0！！！");
		std::exit;
	}
	else if (sampleNum == cloud->points_.size()) 
	{
		open3d::utility::LogWarning("采样点个数与原始点云点数相同！！！");
		std::exit;
	}
	else if (sampleNum > cloud->points_.size()) 
	{
		open3d::utility::LogWarning("采样点个数大于原始点云的点数，无法进行采样！！！");
		std::exit;
	}
	
	std::vector<size_t> selectedIndices(sampleNum);
	const size_t numPoints = cloud->points_.size();
	std::vector<double> distances(numPoints, std::numeric_limits<double>::infinity());
	size_t farthestIndex = 0;
	for (size_t i = 0; i < sampleNum; i++) 
	{
		selectedIndices.push_back(farthestIndex);
		const Eigen::Vector3d& selected = cloud->points_[farthestIndex];
		double maxDist = 0;
#pragma omp parallel for num_threads(20) // 多线程加速
		for (int j = 0; j < numPoints; j++) 
		{
			double dist = (cloud->points_[j] - selected).squaredNorm();
			distances[j] = std::min(distances[j], dist);
			if (distances[j] > maxDist) {
				maxDist = distances[j];
				farthestIndex = j;
			}
		}
	}
	return selectedIndices;
}

std::vector<Eigen::Vector3f> PC2float(PC pc)
{
    std::vector<Eigen::Vector3f> pangolin;

    for(auto& a:pc){
        Eigen::Vector3f changef = {float(a(0)),float(a(1)),float(a(2))};
        pangolin.push_back(changef);
    }
    return pangolin;
}


// #ifdef __linux__
// bool my_mkdir(std::string file_path)
// {
//     if(access(file_path.c_str(), 06))
//    {
//        std::cout << "file_path : (" << file_path << ") didn't exist or no write ability!!" << std::endl;
//        if(mkdir(file_path.c_str(), S_IRWXU))
//        {
//            std::cout << "mkdir " << file_path << " is wrong! please check upper path " << std::endl;
//            exit(0);
//        }
//        std::cout<< "mkdir " << file_path << " success!! " << std::endl;
//    }
// }
// #endif